#include "CANDriver.h"
#include "System\F2806x_Device.h"
#include <string.h>

bool CANDriver::Init()
{
	struct ECAN_REGS ECanaShadow;
	
	// reset data counters
	ResetCounters();
	
	// INIT PINS
	EALLOW;
    GpioCtrlRegs.GPAPUD.bit.GPIO30 = 0;     // Enable pull-up for GPIO30 (CANRXA)
    GpioCtrlRegs.GPAPUD.bit.GPIO31 = 0;     // Enable pull-up for GPIO31 (CANTXA)
    GpioCtrlRegs.GPAQSEL2.bit.GPIO30 = 3;   // Asynch qual for GPIO30 (CANRXA)
    GpioCtrlRegs.GPAMUX2.bit.GPIO30 = 1;    // Configure GPIO30 for CANRXA operation
    GpioCtrlRegs.GPAMUX2.bit.GPIO31 = 1;    // Configure GPIO31 for CANTXA operation
    
    
	/* Configure eCAN RX and TX pins for CAN operation using eCAN regs*/
    ECanaShadow.CANTIOC.all = ECanaRegs.CANTIOC.all;
    ECanaShadow.CANTIOC.bit.TXFUNC = 1;
    ECanaRegs.CANTIOC.all = ECanaShadow.CANTIOC.all;

    ECanaShadow.CANRIOC.all = ECanaRegs.CANRIOC.all;
    ECanaShadow.CANRIOC.bit.RXFUNC = 1;
    ECanaRegs.CANRIOC.all = ECanaShadow.CANRIOC.all;

	/* Configure eCAN for HECC mode - (reqd to access mailboxes 16 thru 31) */
    // HECC mode also enables time-stamping feature
    ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;
    ECanaShadow.CANMC.bit.SCB = 1;
    ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;

	// Set byte orderXXX
	ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;
    ECanaShadow.CANMC.bit.DBO = 1; // LSB first
    ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;

	/* Initialize all bits of 'Message Control Register' to zero */
	// Some bits of MSGCTRL register come up in an unknown state. For proper operation,
	// all bits (including reserved bits) of MSGCTRL must be initialized to zero
    ECanaMboxes.MBOX0.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX1.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX2.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX3.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX4.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX5.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX6.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX7.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX8.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX9.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX10.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX11.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX12.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX13.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX14.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX15.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX16.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX17.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX18.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX19.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX20.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX21.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX22.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX23.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX24.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX25.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX26.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX27.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX28.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX29.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX30.MSGCTRL.all = 0x00000000;
    ECanaMboxes.MBOX31.MSGCTRL.all = 0x00000000;

	// TAn, RMPn, GIFn bits are all zero upon reset and are cleared again
	//  as a matter of precaution.
    ECanaRegs.CANTA.all = 0xFFFFFFFF;   /* Clear all TAn bits */
    ECanaRegs.CANRMP.all = 0xFFFFFFFF;  /* Clear all RMPn bits */
    ECanaRegs.CANGIF0.all = 0xFFFFFFFF; /* Clear all interrupt flag bits */
    ECanaRegs.CANGIF1.all = 0xFFFFFFFF;

	/* Configure bit timing parameters for eCANA*/
    ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;
    ECanaShadow.CANMC.bit.CCR = 1 ;            // Set CCR = 1
    ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;

    // Wait until the CPU has been granted permission to change the configuration registers
    do
    {
      ECanaShadow.CANES.all = ECanaRegs.CANES.all;
    } while(ECanaShadow.CANES.bit.CCE != 1 );       // Wait for CCE bit to be set..

    ECanaShadow.CANBTC.all = 0;
    /* The following block is for 80 MHz SYSCLKOUT. (40 MHz CAN module clock Bit rate = 1 Mbps */
    ECanaShadow.CANBTC.bit.BRPREG = 1;
    ECanaShadow.CANBTC.bit.TSEG2REG = 4;
    ECanaShadow.CANBTC.bit.TSEG1REG = 13;
    ECanaShadow.CANBTC.bit.SAM = 1;
    ECanaRegs.CANBTC.all = ECanaShadow.CANBTC.all;

    ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;
    ECanaShadow.CANMC.bit.CCR = 0 ;            // Set CCR = 0
    ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;

    // Wait until the CPU no longer has permission to change the configuration registers
    do
    {
      ECanaShadow.CANES.all = ECanaRegs.CANES.all;
    } while(ECanaShadow.CANES.bit.CCE != 0 );       // Wait for CCE bit to be  cleared..

	/* Disable all Mailboxes  */
    ECanaRegs.CANME.all = 0;        // Required before writing the MSGIDs

    EDIS;
    
    CANSetupBoxes(this); // callback
	
	// enable All MBOXes
	ECanaRegs.CANME.all = 0xFFFFFFFF;
    
	return true;
}

void CANDriver::SetupMBOX(Uint32 mboxIndex, Uint32 ID, int size, bool isTX)
{
	// NOTE: All IDs are extended->29 BIT!!!
	// select mbox
	volatile struct MBOX *Mailbox;
   	Mailbox = &ECanaMboxes.MBOX0 + mboxIndex;
   	Uint32 mboxBit = (1L << mboxIndex);
   	
   	// Set ID
   	Uint32 msgid = 0x80000000 + ID;
   	Mailbox->MSGID.all = msgid;
   	
   	//set DLC
   	Mailbox->MSGCTRL.bit.DLC = size;
   	
   	// set MBOXes as TX/RX (0-TX, 1-RX)
   	Uint32 canmd = ECanaRegs.CANMD.all;
   	if( isTX ) canmd &= (~mboxBit); // clear bit->TX
   	else canmd |= mboxBit; // set bit->RX
	ECanaRegs.CANMD.all = canmd;
}

void CANDriver::ResetCounters()
{
	m_Sent = 0;
	m_SendAborts = 0;
	m_ReadEmpty = 0;
	m_ReadReceived = 0;
}

bool CANDriver::ReadMBOX(Uint32 mboxIndex, int16* data)
{
	// select mbox
	volatile struct MBOX *Mailbox;
   	Mailbox = &ECanaMboxes.MBOX0 + mboxIndex;
   	
   	// check RMP (is there new data received?)
   	Uint32 mboxBit = (1L << mboxIndex);
   	
   	if( !(ECanaRegs.CANRMP.all & mboxBit) )
   	{
   		// no new data, ABORT
   		m_ReadEmpty++; // stats counter
   		return false;
   	}
   		
   	// read data
   	Uint32 MDL = Mailbox->MDL.all; 
   	Uint32 MDH = Mailbox->MDH.all;
	data[0] = MDL & 0x0000FFFF;
	data[1] = MDL >> 16;
	data[2] = MDH & 0x0000FFFF;
	data[3] = MDH >> 16;
	
	// clear RMP flag
	ECanaRegs.CANRMP.all = mboxBit;
	
	m_ReadReceived++; // stats counter
	
	return true;
}

bool CANDriver::SendMBOX(Uint32 mboxIndex, Uint32 mdl, Uint32 mdh, bool waitAck)
{
	// select mbox
	volatile struct MBOX *Mailbox;
   	Mailbox = &ECanaMboxes.MBOX0 + mboxIndex;
   	Uint32 mboxBit = (1L << mboxIndex);
   	
   	// check if mbox is ready
   	if( ECanaRegs.CANTRS.all & mboxBit ) 
   	{
   		// transfer in progress, ABORT!
   		m_SendAborts++; // stats counter
   		return false;
   	}
   	
   	// fill box
   	Mailbox->MDL.all = mdl;
    Mailbox->MDH.all = mdh;
    
    // send packet
	ECanaRegs.CANTRS.all = mboxBit;  // Set TRS for MBOXx mailbox
	
	// OPTIONAL-> wait for ACK, NOTE: If CAN is disabled/not connected, will block here!!!
	if( waitAck == true )
	{
		while( (ECanaRegs.CANTA.all & mboxBit) == 0 ) {}  // Wait for TAn bit to be set.. 
    	ECanaRegs.CANTA.all = mboxBit;   // Clear TAn
	}
	
	m_Sent++; // stats counter
	
	return true;
}

bool CANDriver::SendMBOX(Uint32 mboxIndex, float data, bool waitAck)
{
	Uint32 mdl;
	memcpy(&mdl, &data, sizeof(float));
	
	return SendMBOX(mboxIndex, mdl, 0, waitAck);
}

bool CANDriver::SendMBOX(Uint32 mboxIndex, float64 data, bool waitAck)
{
	Uint32 dataU[2];
	memcpy(&dataU, &data, sizeof(data));

	return SendMBOX(mboxIndex, dataU[0], dataU[1], waitAck);
}

bool CANDriver::SendMBOX(Uint32 mboxIndex, float data, Uint32 dataI, bool waitAck)
{
	Uint32 mdl;
	Uint32 mdh;
	memcpy(&mdl, &data, sizeof(float));
	memcpy(&mdh, &dataI, sizeof(Uint32));
	
	return SendMBOX(mboxIndex, mdl, mdh, waitAck);
}

bool CANDriver::ReadCANMBOX(Uint32 mboxIndex, float* ptr)
{
	int16 data[4];
	if( ReadMBOX(mboxIndex, data) == false ) return false; // no new data
	
	// new data received, convert bytes0...4 to float!
	memcpy(ptr, data, sizeof(float));
		
	return true;
}

bool CANDriver::ReadCANMBOX(Uint32 mboxIndex, bool* ptr)
{
	int16 data[4];
	if( ReadMBOX(mboxIndex, data) == false ) return false; // no new data
	
	// new data received, convert byte [0] to bool!
	if( data[0] & 0x00FF ) *ptr = true;
	else *ptr = false;
		
	return true;
}
